/**
  ******************************************************************************
  * @file    IMU/algorithm/src/sensor_algorithm.c
  * @author  Zhang Xiaodong
  * @version V0.1.0
  * @date    11-April-2020
  * @brief   sensor utilities for BBIMUv1.0
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; COPYRIGHT 2020 BeyondBits</center></h2>
  *
  *
  ******************************************************************************
 */

 /* Includes ------------------------------------------------------------------*/
#include "sensor_algorithm.h"
#include "QMC5883.h"
#include "inv_app.h"
/** @addtogroup 
  * @{
  */

/** @addtogroup 
  * @{
  */

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
//magnet defines
static fix16_t mag_x_buff[BB_MAF_WINDOWSIZE];
static fix16_t mag_y_buff[BB_MAF_WINDOWSIZE];
static fix16_t mag_z_buff[BB_MAF_WINDOWSIZE];
static fix16_t mag_x_sum;
static fix16_t mag_y_sum;
static fix16_t mag_z_sum;

/* Private function prototypes -----------------------------------------------*/
  
/* Private functions ---------------------------------------------------------*/
static float bb_fix_to_float(fix16_t data);
static fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1);

//code begin

/**
  * @brief  algorithm init
  * @param  void
  * @retval bbstatus
*/
BB_StatusTypeDef bb_alg_init(void)
{
  BB_StatusTypeDef status = BB_NO_ERROR;
  int i;
  //float mag_trans_fac = -1;
  int16_t mag_raw[3];
  fix16_t mag_maf[3];
//magnet initial
  memset(mag_x_buff,0,BB_MAF_WINDOWSIZE);
	memset(mag_y_buff,0,BB_MAF_WINDOWSIZE);
	memset(mag_z_buff,0,BB_MAF_WINDOWSIZE);

	mag_x_sum = 0;
	mag_y_sum = 0;
	mag_z_sum = 0;

//inv initial

//barometer initial

//program begin

  //get the magnet mafval from data
  for(i = 0;i<BB_MAF_WINDOWSIZE;i++)
  {
    status = QMC5883_GetData(mag_raw);
    
    if( status != BB_NO_ERROR)
      {
        BB_MSG(BB_MSG_LEVEL_ERROR,"QMC5883 Get Data ERROR");
        return status;
      }

    if(i == (BB_MAF_WINDOWSIZE - 1))
      status = bb_maf(mag_raw,mag_maf,1);
    else 
      status = bb_maf(mag_raw,mag_maf,0);
  }

  BB_MSG(BB_MSG_LEVEL_INFO,"Mag MAF(uT):x=%f,y =%f,z=%f",bb_fix_to_float(mag_maf[0]),
                          bb_fix_to_float(mag_maf[1]),bb_fix_to_float(mag_maf[2]));
  //get inv bias
  GetInvBias();

  return status;
}

/**
  * @brief  Moving average filter
  * @param  int16_t* MagnetRawAd,fix16_t* mag_maf
  * @retval bbstatus
*/
BB_StatusTypeDef bb_maf(int16_t* MagnetRawAd,fix16_t* mag_maf,uint8_t maf_en)
{
  BB_StatusTypeDef status = BB_NO_ERROR;
  float factor = -1;
  // float mag_raw_ut[3];
  // memset(mag_raw_ut,0,3);

  factor = get_mag_trans_fac();
	if(factor == -1)
	{	
		BB_MSG(BB_MSG_LEVEL_ERROR,"FSR setting is wrong");
		return BB_ERROR_UNEXPECTED;
	}

	//get real value in uT from raw data
	// mag_raw_ut[0] = (float)MagnetRawAd[0] * BBALGSCALE * factor * BB_FIX_ONE;//unit:ut
	// mag_raw_ut[1] = (float)MagnetRawAd[1] * BBALGSCALE * factor * BB_FIX_ONE;//unit:ut
	// mag_raw_ut[2] = (float)MagnetRawAd[2] * BBALGSCALE * factor * BB_FIX_ONE;//unit:ut
 
	//maf
	for (int16_t i = 0; i < BB_MAF_WINDOWSIZE; i++)
	{
		if(i == (BB_MAF_WINDOWSIZE - 1))
			{
				mag_x_buff[i] = (float)MagnetRawAd[0] * factor * BBALGSCALE * BB_FIX_ONE;
				mag_y_buff[i] = (float)MagnetRawAd[1] * factor * BBALGSCALE * BB_FIX_ONE;
				mag_z_buff[i] = (float)MagnetRawAd[2] * factor * BBALGSCALE * BB_FIX_ONE;
			}
		else 
			{
				mag_x_buff[i] = mag_x_buff[i+1];
				mag_y_buff[i] = mag_y_buff[i+1];
				mag_z_buff[i] = mag_z_buff[i+1];
			}
	}
	
	mag_x_sum = mag_x_sum  + mag_x_buff[BB_MAF_WINDOWSIZE-1]- mag_x_buff[0];
	mag_y_sum = mag_y_sum  + mag_y_buff[BB_MAF_WINDOWSIZE-1]- mag_y_buff[0];
	mag_z_sum = mag_z_sum  + mag_z_buff[BB_MAF_WINDOWSIZE-1]- mag_z_buff[0];

	//mag_x_maf = mag_x_sum / BB_MAF_WINDOWSIZE
  if(maf_en)
  {
	  mag_maf[0] = fix16_mul(mag_x_sum,BB_MAF_FACTOR);//
	  mag_maf[1] = fix16_mul(mag_y_sum,BB_MAF_FACTOR);
	  mag_maf[2] = fix16_mul(mag_z_sum,BB_MAF_FACTOR);
  }
  else
  {
    mag_maf[0] = 0;
    mag_maf[1] = 0;
    mag_maf[2] = 0;
  }
  
  return status;
}

/**
  * @brief  fusion algorithm from accel,gyro and magnet base on Linear Kalman Filter
  * @param   - int16_t accel_x,
  *          - int16_t accel_y,
  *          - int16_t accel_z,
  *          - 
  *          - int16_t gyro_x,
  *          - int16_t gyro_y,
  *          - int16_t gyro_z,
 
  *          - int16_t mag_y,
  *          - int16_t mag_y,
  *          - int16_t may_y
  * @retval 0: calibration is ongoing
            1: count calibration done
            2: Dont do calibration ,just set cali to ZERO
  */

 BB_StatusTypeDef Linear_KF(
                            int16_t accel_x,
                            int16_t accel_y,
                            int16_t accel_z,
                            
                            int16_t gyro_x,
                            int16_t gyro_y,
                            int16_t gyro_z,

                            int16_t mag_x,
                            int16_t mag_y,
                            int16_t may_z)
 {
//TODO finish it!!!
 }


static float bb_fix_to_float(fix16_t data)
{
  return ((float)(data) / BBALGSCALE / ((float)BB_FIX_ONE));
}

static fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1)
{
	// Each argument is divided to 16-bit parts.
	//					AB
	//			*	 CD
	// -----------
	//					BD	16 * 16 -> 32 bit products
	//				 CB
	//				 AD
	//				AC
	//			 |----| 64 bit product
	int32_t A = (inArg0 >> 16), C = (inArg1 >> 16);
	uint32_t B = (inArg0 & 0xFFFF), D = (inArg1 & 0xFFFF);
	
	int32_t AC = A*C;
	int32_t AD_CB = A*D + C*B;
	uint32_t BD = B*D;
	
	int32_t product_hi = AC + (AD_CB >> 16);
	
	// Handle carry from lower 32 bits to upper part of result.
	uint32_t ad_cb_temp = AD_CB << 16;
	uint32_t product_lo = BD + ad_cb_temp;
	if (product_lo < BD)
		product_hi++;
	
#ifndef FIXMATH_NO_OVERFLOW
	// The upper 17 bits should all be the same (the sign).
	if (product_hi >> 31 != product_hi >> 15)
		return 0x80000000;
#endif
	
#ifdef FIXMATH_NO_ROUNDING
	return (product_hi << 16) | (product_lo >> 16);
#else
	// Subtracting 0x8000 (= 0.5) and then using signed right shift
	// achieves proper rounding to result-1, except in the corner
	// case of negative numbers and lowest word = 0x8000.
	// To handle that, we also have to subtract 1 for negative numbers.
	uint32_t product_lo_tmp = product_lo;
	product_lo -= 0x8000;
	product_lo -= (uint32_t)product_hi >> 31;
	if (product_lo > product_lo_tmp)
		product_hi--;
	
	// Discard the lowest 16 bits. Note that this is not exactly the same
	// as dividing by 0x10000. For example if product = -1, result will
	// also be -1 and not 0. This is compensated by adding +1 to the result
	// and compensating this in turn in the rounding above.
	fix16_t result = (product_hi << 16) | (product_lo >> 16);
	result += 1;
	return result;
#endif
}
 /**
  * @}
  */

/**
  * @}
  */

/************************ (C) COPYRIGHT BeyondBits *****END OF FILE****/
